
Optional Matrix-math.F

Requires FLOAT-TOOLS
Requires A
Requires FixedPoint
Requires OpenGL
Requires GL-Tools

Only Forth Definitions

{ ==================================================================
3D matrix math


Derived from Charles Melice's 3d matrix package V1.4

Designed to be compatible with OpenGL

This version uses DOUBLE PRECISION 4x4 OpenGL matrix format.

Surprisingly, double precision is faster than single precision (i've tested it and have deleted the file since it's no longer needed )

MTRANSLATE, MSCALE, and MROTATE are extremely slow.  It's recommended
to use OpenGL's matrix routines and glGetDoublev as they are at least 33% faster.

M[][]   Access cells of a matrix ( col row -- addr )
M.   Print the contents of a matrix.
MADD   Add two matrices, place the sum in first.
MCOPY   Copy one matrix to another.
MIDENTITY  Load the identity matrix into a matrix.
MTRANSFORM - translated from Pablo Reda's - multiply fixed point vector by DOUBLE-PRECISION
          4x4 OpenGL matrix.

MTRANSLATE
MSCALE
MRotate

More to be added as needed...
==================================================================== }

128 Constant /Matrix
0e FVALUE fx
0e FVALUE fy
0e FVALUE fz
\ 0e FVALUE co
\ 0e FVALUE si
\ 0e FVALUE ct

Create iMtx
   1.0e f,  0e f,    0e f,    0e f,
   0e f,    1.0e f,  0e f,    0e f,
   0e f,    0e f,    1.0e f,  0e f,
   0e f,    0e f,    0e f,    1.0e f,


\ create tempMtx /Matrix /allot
\ ====================================================================

create ftempx  0 , 0 ,
create ftempy  0 , 0 ,

\ M[][]  -  A tool for calculating offsets of matrix cells.
: M[][]   ( col row -- ofs ) 2 lshift + 3 lshift ;

\ F+@ - for indexing into matrices using the A register.
: F+@     a@ + F@ ;
-? : F+@     POSTPONE a@ POSTPONE + POSTPONE F@ ; IMMEDIATE

: 3.>FVALUES   .>F fto FZ  .>F fto FY  .>F fto FX ;

\ ---------------------------------------------------------------------

: M.   4 0 do cr 4 0 do f@+ f. space loop loop drop ;

: M!    /Matrix MOVE ;

: MIDENTITY   iMtx swap M! ;

: Matrix:   create here midentity /matrix allot ;

: MAdd   ( m1 m2 -- m1' )   over swap 16 0 DO 2dup f@ f+! 8 8 2+ LOOP 2drop ;

{
: MTransform ( x. y. z.  addr  -- x. y. z. )
  A!
   3s>f  ( f: x y z )
   fthird 0 F+@ f* fthird 32 F+@ f* f+ fover 64 F+@ f* f+ 96 F+@ f+ f>s
   fthird 8 F+@ f* fthird 40 F+@ f* f+ fover 72 F+@ f* f+ 104 F+@ f+ f>s
     frot 16 F+@ f* frot  48 F+@ f* f+ fswap 80 F+@ f* f+ 112 F+@ f+ f>s ;
}

\ : _mtransform
\    fthird dup f@ f* fthird 32 + dup f@ f* f+ fover 32 + dup f@ f* f+ 32 + f@ f+ f>s swap ;
: MTransform ( x y z  addr  -- x y z )  \ integer or fixed point is OK.
  >r
   3s>f  ( f: x y z )
  r>
  dup
   fthird dup f@ f* fthird 32 + dup f@ f* f+ fover 32 + dup f@ f* f+ 32 + f@ f+ f>s swap
  8 + dup
   fthird dup f@ f* fthird 32 + dup f@ f* f+ fover 32 + dup f@ f* f+ 32 + f@ f+ f>s swap
  8 +
     frot dup f@ f* frot 32 + dup f@ f* f+ fswap 32 + dup f@ f* f+ 32 + f@ f+ f>s ;


{
: .m[]@   a@ + f@ f>. ;
\ : .m[]*   a@ + f@ s>f f* f>s ;
: .m[]*   a@ + f@ f>. .* ;

: MTransform. ( x. y. z.  addr  -- x. y. z. )
  A!
   locals| z y x |
   x 0 .m[]*  y 32 .m[]* + z 64 .m[]* +  96 .m[]@ +
   x 8 .m[]*  y 40 .m[]* + z 72 .m[]* +  104 .m[]@ +
   x 16 .m[]* y 48 .m[]* + z 80 .m[]* +  112 .m[]@ + ;
}

: MTranslate ( x. y. z. addr -- )  >r 3.>f r> 112 + dup F+! 8 - dup F+! 8 - F+! ;


: *ROW+   FX A@ F@ F* F!a+
          FY A@ F@ F* F!a+
          FZ A@ F@ F* F!a+
          8 +A ;

: MScale   ( x. y. z. addr -- ) A!  3.>FVALUES *ROW+ *ROW+ *ROW+ ;

\ This is a workaround as rotation is really complicated
: MRotate ( x. y. z. angle. addr -- )
   >r
   push-modelview
   r@ glLoadMatrixd
   .>f 3s>f glRotatef
   GL_MODELVIEW_MATRIX r> glGetDoublev
   pop-modelview
  ;

: gl-get-matrix
  glGetDoublev ;

\ ====================================================================
